#include "../../../XMLTools.h"
#include "../../../MathTools.h"

#include "ModelPoseSensorMeasurement.h"

using namespace MMM;

ModelPoseSensorMeasurement::ModelPoseSensorMeasurement(float timestep, const Eigen::Vector3f &rootPosition, const Eigen::Vector3f &rootRotation, SensorMeasurementType type) :
    InterpolatableSensorMeasurement(timestep, type),
    rootPosition(rootPosition),
    rootRotation(rootRotation)
{
}

ModelPoseSensorMeasurement::ModelPoseSensorMeasurement(float timestep, const Eigen::Matrix4f &rootPose, SensorMeasurementType type) :
    InterpolatableSensorMeasurement(timestep, type) {
    Eigen::VectorXf p = Math::matrix4fToPoseRPY(rootPose);
    rootPosition = p.head(3);
    rootRotation = p.tail(3);
}

SensorMeasurementPtr ModelPoseSensorMeasurement::clone() {
    return clone(timestep);
}

bool ModelPoseSensorMeasurement::equals(SensorMeasurementPtr sensorMeasurement) {
    ModelPoseSensorMeasurementPtr ptr = boost::dynamic_pointer_cast<ModelPoseSensorMeasurement>(sensorMeasurement);
    if (ptr) {
        return rootPosition == ptr->rootPosition && rootRotation == ptr->rootRotation;
    }
    else return false;
}

ModelPoseSensorMeasurementPtr ModelPoseSensorMeasurement::clone(float newTimestep) {
    ModelPoseSensorMeasurementPtr clonedSensorMeasurement(new ModelPoseSensorMeasurement(newTimestep, rootPosition, rootRotation, type));
    return clonedSensorMeasurement;
}

void ModelPoseSensorMeasurement::appendMeasurementDataXML(RapidXMLWriterNodePtr measurementNode) {
    measurementNode->append_node("RootPosition")->append_data_node(XML::toString(rootPosition));
    measurementNode->append_node("RootRotation")->append_data_node(XML::toString(rootRotation));
}

Eigen::Vector3f ModelPoseSensorMeasurement::getRootPosition() {
    return rootPosition;
}

Eigen::Vector3f ModelPoseSensorMeasurement::getRootRotation() {
    return rootRotation;
}

Eigen::Matrix4f ModelPoseSensorMeasurement::getRootPose() {
    return Math::poseRPYToMatrix4f(rootPosition, rootRotation);
}

ModelPoseSensorMeasurementPtr ModelPoseSensorMeasurement::interpolate(ModelPoseSensorMeasurementPtr other, float timestep, SensorMeasurementType type) {
    Eigen::Vector3f interpolatedRootPosition = Math::linearInterpolation(this->rootPosition, this->timestep, other->rootPosition, other->timestep, timestep);
    Eigen::Vector3f interpolatedRootRotation = Math::linearInterpolation(this->rootRotation, this->timestep, other->rootRotation, other->timestep, timestep);
    ModelPoseSensorMeasurementPtr interpolatedSensorMeasurement(new ModelPoseSensorMeasurement(timestep, interpolatedRootPosition, interpolatedRootRotation, type));
    return interpolatedSensorMeasurement;
}
